#include "cdynamicwindowapproach.h"
#include <QDebug>
/**
 * @brief 执行动态窗口法规划路径
 * @param waypoints 航点经纬度   @details 程序一开始先在地图上点击几个航迹点以形成粗略的航线，按照起点到终点的顺序依次存放
 */
vector<SParameterOfWindow> CDynamicWindowApproach::PlanPath(vector<pair<double, double> > &waypoints)
{
    vector<pair<double, double>> convertedWaypoints;
    for(auto waypoint : waypoints)
    {
        convertedWaypoints.emplace_back(getAngleDist(waypoint.first, waypoint.second, m_startPos.m_lng, m_startPos.m_lat));
    }
    vector<SParameterOfWindow> pathWindow;
    SParameterOfWindow initWindow(0, 0, *m_phyParameter);
    pathWindow.emplace_back(initWindow);
    while(1)
    {
        if(abs(m_x - convertedWaypoints.back().first) < 5 && abs(m_y - convertedWaypoints.back().second) < 5)
        {
            qDebug() << QString("已经到达终点");
            return pathWindow;
        }
        //Todo:获取障碍物距离信息,pair<double, double>存放相对距离，first对应x方向，second对应y方向;
        vector<pair<double, double>> obstacles;
        vector<double> nextPara = DynamicWindowApproch(initWindow, obstacles, waypoints);
        initWindow = CalculateNextWindowParameter(initWindow, nextPara[0], nextPara[1], nextPara[2]/*, obstacles, waypoints*/);
        pathWindow.push_back(initWindow);
    }
}

vector<double> CDynamicWindowApproach::DynamicWindowApproch(SParameterOfWindow currentState, vector<pair<double, double> > &obstacles, vector<pair<double, double> > &waypoints)
{
    vector<double> solutionSpace = CreateSolutionSpace(m_phyParameter);
    double sumHeading = 0;
    double sumClearance = 0;
    double sumVelocity = 0;
    vector<SEvaluationParameter> evalParas;
    for(double u = solutionSpace[0]; u < solutionSpace[1]; u += m_samplingVelocity)
    {
        for(double v = solutionSpace[2]; v < solutionSpace[3]; v += m_samplingVelocity)
        {
            for(double r = solutionSpace[4]; r < solutionSpace[5]; r += m_samplingAngularVelocity)
            {
                vector<SParameterOfWindow> trajectories = GenerateTraj(currentState, u, v, r/*, obstacles, waypoints*/);
                SEvaluationParameter tmpEvalPara;
                double heading = CalculateDisOfTarget(trajectories.back().m_x, trajectories.back().m_y, waypoints);
                double clearance = CalculateDisOfObstacle(trajectories.back().m_x, trajectories.back().m_y, obstacles);
                double stopDist = CalculateBreakingDist(u, v);
                if(clearance /*trajectories.back().m_distOfObstacle*/ < stopDist)
                {
                    continue;
                }
                tmpEvalPara.m_heading = heading /*trajectories.back().m_distOfTarget*/;
                tmpEvalPara.m_clearance = clearance /*trajectories.back().m_distOfObstacle*/;
                tmpEvalPara.m_velocity = sqrt(pow(u, 2) + pow(v, 2));
                tmpEvalPara.m_u = u;
                tmpEvalPara.m_v = v;
                tmpEvalPara.m_r = r;
                sumHeading += tmpEvalPara.m_heading;
                sumClearance += tmpEvalPara.m_clearance;
                sumVelocity += tmpEvalPara.m_velocity;
                evalParas.emplace_back(tmpEvalPara);
            }
        }
    }
    double maxG = 0;
    double selectedU = 0;
    double selectedV = 0;
    double selectedR = 0;
    for(auto item : evalParas)
    {
        double smoothHeading = item.m_heading/sumHeading;
        double smoothClearance = item.m_clearance/sumClearance;
        double smoothVelocity = item.m_velocity/sumVelocity;
        double tmpG = WEIGHT_HEADING/smoothHeading + WEIGHT_CLEARANCE*smoothClearance + WEIGHT_VELOCITY*smoothVelocity;
        if(tmpG > maxG)
        {
            maxG = tmpG;
            selectedU = item.m_u;
            selectedV = item.m_v;
            selectedR = item.m_r;
        }
    }
    vector<double> nextPara(3);
    nextPara[0] = selectedU;
    nextPara[1] = selectedV;
    nextPara[2] = selectedR;
    return nextPara;
}

/**
 * @brief 计算速度u/v与角速度r形成的解空间
 * @details 依据最大加速度、最大减速度可以分别计算速度的理论上下限，
 * 但是必须符合速度的物理约束限制：对于下限，两者取最大；对于上限，两者取最小
 * @return vector数组，其中0,1元素表示速度u的下限和上限；
 * 2和3元素表示速度v的下限和上限；
 * 4和5元素表示角速度r的下限和上限
 */
vector<double> CDynamicWindowApproach::CreateSolutionSpace(SParameterOfPhysicalConstraint *phyParameter)
{
    vector<double> solutionSpace(6, 0);
    double tmpMinUVelocity = phyParameter->m_u - m_limit->m_dUMax * m_samplingRate;
    double tmpMaxUVelocity = phyParameter->m_u + m_limit->m_aUMax * m_samplingRate;
    double tmpMinVVelocity = phyParameter->m_v - m_limit->m_dVMax * m_samplingRate;
    double tmpMaxVVelocity = phyParameter->m_v + m_limit->m_aVMax * m_samplingRate;
    double tmpMinAngularVelocity = phyParameter->m_r - m_limit->m_angularDMax * m_samplingRate;
    double tmpMaxAngularVelocity = phyParameter->m_r + m_limit->m_angularAMax * m_samplingRate;
    solutionSpace.emplace_back(tmpMinUVelocity < m_limit->m_uMin ? m_limit->m_uMin : tmpMinUVelocity);
    solutionSpace.emplace_back(tmpMaxUVelocity > m_limit->m_uMax ? m_limit->m_uMax : tmpMaxUVelocity);
    solutionSpace.emplace_back(tmpMinVVelocity < m_limit->m_vMin ? m_limit->m_vMin : tmpMinVVelocity);
    solutionSpace.emplace_back(tmpMaxVVelocity > m_limit->m_vMax ? m_limit->m_vMax : tmpMaxVVelocity);
    solutionSpace.emplace_back(tmpMinAngularVelocity < m_limit->m_rMin ? m_limit->m_rMin : tmpMinAngularVelocity);
    solutionSpace.emplace_back(tmpMaxAngularVelocity > m_limit->m_rMax ? m_limit->m_rMax : tmpMaxAngularVelocity);
    return solutionSpace;
}
/**
 * @brief 根据给定的解（ u/v/r）生成下一个动态窗口
 * @param currentWindow 当前物理量信息
 * @param u  下一窗口的横向速度
 * @param v  下一窗口的纵向速度
 * @param r  下一窗口的角速度
 * @return 下一个窗口信息
 */
SParameterOfWindow CDynamicWindowApproach::CalculateNextWindowParameter(const SParameterOfWindow currentWindow, const double u, const double v, const double r/*, const vector<pair<double, double> > &obstacles, const vector<pair<double, double> > &waypoints*/)
{
    SParameterOfWindow nextWindow = currentWindow;
    nextWindow.m_x += u*m_samplingRate;
    nextWindow.m_y += v*m_samplingRate;
    nextWindow.m_phyParameter.m_psi += r*m_samplingRate;
    nextWindow.m_phyParameter.m_r = r;
    nextWindow.m_phyParameter.m_u = u;
    nextWindow.m_phyParameter.m_v = v;
//    nextWindow.m_distOfObstacle = CalculateDisOfObstacle(nextWindow.m_x, nextWindow.m_y, obstacles);
//    nextWindow.m_distOfTarget = CalculateDisOfTarget(nextWindow.m_x, nextWindow.m_y, waypoints);
    return nextWindow;
}

/**
 * @brief 计算（x,y）与障碍物之间的最短距离
 * @param x  当前横向方向的坐标
 * @param y  当前纵向方向的坐标
 * @param obstacles  vector数组形式存放雷达探测到的所有障碍物距离当前位置的相对距离，以(x,y)形式存放至数组
 * @return 最短距离
 */
double CDynamicWindowApproach::CalculateDisOfObstacle(double x, double y, const vector<pair<double, double> > &obstacles)
{
    double minDisOfObstacles = INF;
    for(auto item : obstacles)
    {
        double convertX = item.first + m_x;
        double convertY = item.second + m_y;
        double dis = sqrt(pow(convertX - x, 2) + pow(convertY - y, 2));
        if(dis < minDisOfObstacles)
            minDisOfObstacles = dis;
    }
    return minDisOfObstacles;
}

double CDynamicWindowApproach::CalculateDisOfTarget(double x, double y, const vector<pair<double, double> > &waypoints)
{
    //Todo:
    //step1:根据航点相对坐标可以计算航线的函数表达式
    //step2:根据函数表达式，计算预测航迹点距离直线的距离
    //step3:求最小距离
}
/**
 * @brief 预测轨迹点
 * @param currentWindow    当前物理量信息
 * @param u 预测时间内的横向速度
 * @param v 预测时间内的纵向速度
 * @param r 预测时间内的角速度
 * @return  预测时间内的轨迹点
 */
vector<SParameterOfWindow> CDynamicWindowApproach::GenerateTraj(const SParameterOfWindow &currentWindow, const double u, const double v, const double r/*, const vector<pair<double, double> > &obstacles, const vector<pair<double, double> > &waypoints*/)
{
    SParameterOfWindow tmpWindow = currentWindow;
    vector<SParameterOfWindow> trajectories;
    trajectories.emplace_back(tmpWindow);
    for(int i = 0; i < m_wcount; i++)
    {
        tmpWindow = CalculateNextWindowParameter(tmpWindow, u, v, r/*, obstacles, waypoints*/);
        trajectories.emplace_back(tmpWindow);
    }
    return trajectories;
}

/**
 * @brief 计算制动距离
 * @details  横向和纵向分别按照最大减速度进行减速产生的制动距离（停止距离）
 * @param u 当前横向速度
 * @param v 当前纵向速度
 * @return 制动距离
 */
double CDynamicWindowApproach::CalculateBreakingDist(double u, double v)
{
    double x = 0;
    double y = 0;
    while(u > 0 || v > 0)
    {
        x += u*m_samplingRate;
        y += v*m_samplingRate;
        u > 0 ? u -= m_limit->m_dUMax*m_samplingRate : 1;
        v > 0 ? v -= m_limit->m_dVMax*m_samplingRate : 1;
    }
    return sqrt(pow(x, 2) + pow(y, 2));
}



